/**
 * anis.darwich
 */

package clubhouse.base;

import java.text.DecimalFormat;
import java.util.ListIterator;
import java.util.Vector;

import org.apache.log4j.Logger;
import org.apache.log4j.PropertyConfigurator;


import clubhouse.antislam.AntiSLAM;
import clubhouse.communication.ReceptionFifo;
import clubhouse.communication.RobotCommunication;
import clubhouse.communication.TransmissionFifo;
import clubhouse.control.DataChecker;
import clubhouse.control.ErrorRetriever;
import clubhouse.control.LandmarkGenerator;
import clubhouse.error.TransmissionError;
import clubhouse.kalmanfilter.Ekf;
import clubhouse.motioncommand.MotionCommand;
import clubhouse.motioncommand.MotionPlan;
import clubhouse.motioncommand.NodeControl;
import clubhouse.visualisation.WSNexperimentation;

public class Base implements Runnable {
	
	public static final boolean DEBUG = false;
	private final static Logger log = Logger.getLogger(Base.class);
	public static final double[] finalDestination = new double[]{60,15};

	private int errorFactor;
	private int MODE = 0;
	Runnable xbee;
	private Thread t;
	private boolean simulation = true;
	private int scenario;
	private boolean antiSlam;
	private boolean enableError;
	private boolean enableEkf;
	private boolean completeSimulation;
	
	private MotionPlan motionControl;
	private TransmissionError mistaker;
	private AntiSLAM counterSlam;
	
	private Ekf filter;
	
	public ReceptionFifo reception;
	public TransmissionFifo transmission;
	
	public Robot rover;
	public Landmarks nodes;
	public LandmarkGenerator landmarkGenerator = new LandmarkGenerator();;
	
	public WSNexperimentation visual;
	public ErrorRetriever storeError;
	
	private Vector<double[]> generatedLandmarks;
	
	public Base(int mode, WSNexperimentation v) {
		this.MODE = mode;
		this.visual = v;
	}
	
	public void setSimulation(boolean simul) {
		this.simulation = simul;
	}
	
	public void setAntiSlam(boolean enableAntiSlam) {
		this.antiSlam = enableAntiSlam;
	}
	
	public void setError(boolean enabledError) {
		this.enableError = enabledError;
	}
	
	public void setErrorFactor(int error) {
		this.errorFactor = error;
	}
	
	public void setScenario(int scenario) {
		this.scenario = scenario;
	}
	
	public void setEkf(boolean enableEkf) {
		this.enableEkf = enableEkf;
	}
	
	public void setCompleteSimulation(boolean simulation) {
		this.completeSimulation = simulation;
	}
	
	public void initError() {	
		this.mistaker = new TransmissionError();
		this.storeError = new ErrorRetriever();
		this.mistaker.setErrorFactor(this.errorFactor);
	}
	
	public void initEkf() {
		System.out.println("Creating filter");
		this.filter = new Ekf();
		System.out.println("Initialising filter");
		this.filter.initCalcul();
		System.out.println("Setting initial position");
		this.filter.setInitialPosition(rover.getX(), rover.getY(), 0, 0);
		System.out.println("Exiting init EKF");
	}
	
	public void initAntiSlam(){	
		this.counterSlam = new AntiSLAM(this.scenario);
	}
	
	public void initMotion() {
		this.motionControl = new MotionPlan(this.scenario);
	}
	
	public void initMain() {
		/*initialize robot's position*/
		if (this.scenario == 1){
			this.rover = new Robot(-10,20,0);
		} else {
			this.rover = new Robot(-20,30,0);
		}

		/*initialize known landmarks*/
		this.nodes = new Landmarks();
	}
	
	public void initCommunication(){
		try {
			this.reception = new ReceptionFifo();
			this.transmission = new TransmissionFifo();
			Runnable xbee = new RobotCommunication(this.reception, this.transmission);
			this.t = new Thread (xbee);
			this.t.start();
		} catch (Exception e) {
			e.printStackTrace();
		}
	}
	
	public void manageLandmarks(int action, String fileName, int numberOfNodes, Vector<double[]> userLandmarks) {

		switch (action) {
		case 0:
			//random
			this.landmarkGenerator.setNumberOfNode(numberOfNodes);
			this.generatedLandmarks = this.landmarkGenerator.generateScenario();
			break;
		case 1:
			//read from file
			this.generatedLandmarks = this.landmarkGenerator.readScenario(fileName);
			break;
		case 2:
			//read from panel
			this.generatedLandmarks = userLandmarks;
			break;
		case 3:
			//save current configuration
			break;
		default:
			break;
		}
	}
	
	public void stopRoutine() {
		//to do...
		setSimulation(false);
	}
	
	public void clearRoutine() {
		//to do...
		stopRoutine();
		//((RobotCommunication) xbee).close();
		//this.t.interrupt();
	}
	
	public void resumeRoutine() {
		setSimulation(true);
		//this.t.notify();
	}
		
	public void run() {
		startRoutine();
	}	
	
	public void startRoutine() {
		PropertyConfigurator.configure("log4j.properties");
		
		/*enter operating mode*/
		switch (MODE) {
		case 0:
			//communication, motioncommand, error and ekf
			try {
				this.executeRoutine0();
			} catch (Exception e) {
				log.debug("");
				e.printStackTrace();
			}
			break;
		case 1:
			//communication and motioncommand
			try {
				this.executeRoutine1();
			} catch (Exception e) {
				log.debug("");
				e.printStackTrace();
			}
			break;
		case 2:
			try {
				executeRoutine2();
			} catch (Exception e) {
				log.debug("");
				e.printStackTrace();
			}
			break;
		default:
			//
			break;
		}
	}	
	
	public void executeRoutine0() throws Exception {
		while(simulation) {
			double[] increment;
			int i;
			
			/*get next motion command*/
			//motionControl.getNextPulses(rover.getEstimateX(), rover.getEstimateY(), rover.getEstimateR(), this.nodes);
			
			/*send motion command*/
			MotionCommand motionCmd = new MotionCommand(16, motionControl.getPulse_dist(), motionControl.getPulse_rot(), motionControl.getTrigo());
			transmission.txMotionCom(motionCmd);
			
			/*wait for ack*/
			Thread.sleep(1000);
			MotionCommand motionAck = reception.getMotionAck();
			if (motionAck != null) {
				if ((motionAck.getDistancePulse() == motionCmd.getDistancePulse()) && (motionAck.getRotationPulse() == motionCmd.getRotationPulse())) {
					log.debug("");
				} else {
					continue;
				}
			} else {
				continue;
			}
			
			/*apply ekf (motion)*/
			//double[] lastPosition = rover.getLastPosition();
			increment = motionAck.getCoordIncrement();
			rover.setPosition(rover.getX()+increment[0], rover.getY()+increment[1], rover.getr()+increment[2]);
			rover.storePosition();
			//intermediateEstimate = filter.applyMotion(rover.getEstimateX(), rover.getEstimateY(), rover.getEstimateR(), lastPosition[0], lastPosition[1], lastPosition[2]);
			
			/*listen to nodes*/
			Thread.sleep(5000);
			Vector<NodeControl> receivedMsg = reception.getReceivedLandmarks();
			NodeControl nodeMsg;
			
			for(i = 0; i < receivedMsg.size(); i++) {
				nodeMsg = receivedMsg.elementAt(i);
				if (nodes.knownLandmark(nodeMsg.getSource(), nodeMsg.getX(), nodeMsg.getY())) {
					log.debug("");
				} else {
					nodes.storeLandmark(nodeMsg.getSource(), nodeMsg.getX(), nodeMsg.getY());
				}
				
				/*apply error*/
				//double[] measurement = mistaker.applyError(rover.getEstimateX(), rover.getEstimateY(), nodeMsg.getX(), nodeMsg.getY());
				
				/*apply ekf (measurement)*/
				//filter.applyMeasurement(nodeMsg.getSource(), measurement[0], measurement[1], rover.getEstimateX(), rover.getEstimateY(), rover.getEstimateR(), intermediateEstimate);
			}
			
			/*get kf estimate*/
			//estimate = filter.getEstimates();
			//rover.setEstimatePosition(estimate[0], estimate[1], estimate[2]);
			rover.storeEstimatePosition();
		}
	}
	
	public void executeRoutine1() throws Exception {
		double [] tempError = null;
		double [] tempLandmark = null;
		double [] receiveCoord = new double[2];
		double [] antiSlamedCoord = new double[2];
		double [] errCoord, finalCoord;
		ListIterator<double[]> li;
		double distance, rotation, trigo;
		int coordIndex = 0;
		DecimalFormat df = new DecimalFormat("#####.##");
		
		this.landmarkGenerator.setNumberOfNode(15);
		this.generatedLandmarks = this.landmarkGenerator.generateScenario();
		
		try {
			/*********************motion plan********************/
			while (euclideanDist(this.rover.getX(),this.rover.getY(),Base.finalDestination[0],Base.finalDestination[1])>3) { //end
				if (Base.DEBUG) System.out.println("(executeRoutine1) Motion Plan");
				if (!this.simulation){
					Thread.sleep(1000);
				} else {
					this.visual.addDebugLog2(DataChecker.initialPosition, this.rover.getX(),this.rover.getY(),this.rover.getr());
					ListIterator<double[]> landmarkList = this.generatedLandmarks.listIterator();
					while (landmarkList.hasNext()) {
						double[] landmark = landmarkList.next();
						if ((euclideanDist(this.rover.getX(),this.rover.getY(),landmark[1],landmark[2])<=DataChecker.communicationRange)&&(!this.nodes.knownLandmark((int)landmark[0], landmark[1], landmark[2]))) this.nodes.storeLandmark((int)landmark[0], landmark[1], landmark[2]);	
					}
					
					for (int j=0;j<this.nodes.size();j++){
						tempError = this.nodes.realLandmarks.get(j);
						//Receiving nodes' coord
						coordIndex = (int)tempError[0];
						receiveCoord[0] = tempError[1];
						receiveCoord[1] = tempError[2];
					
						//Apply Anti-SLAM error
						antiSlamedCoord = receiveCoord;
					
						//Apply error
						errCoord = antiSlamedCoord;
						//Apply EKF - not supported /!\/!\/!\/!\
						finalCoord = errCoord;
						//Store final Coord
						this.nodes.replaceLandmark(coordIndex, finalCoord[0], finalCoord[1]);
						tempError = null;
					}
		        
					//Display all landmarks
					li = this.nodes.getLandmarks().listIterator();
					while(li.hasNext()){
						tempLandmark = li.next();
						this.visual.addDebugLog2(DataChecker.landmark,tempLandmark[0],this.nodes.size(),tempLandmark[1],tempLandmark[2]);
					}
				
					//Apply EKF
					
					if (Base.DEBUG) System.out.println("(executeRoutine1) We have "+this.nodes.size()+" landmarks");
					this.motionControl.getNextPulses(this.rover.getX(),this.rover.getY(),this.rover.getr(),this.nodes);
					distance = this.motionControl.getPulse_dist();
					rotation = this.motionControl.getPulse_rot();
					trigo = this.motionControl.getTrigo();
				
					/*****Logs*****/
					this.visual.addDebugLog2(DataChecker.destination,this.motionControl.getDestinationCoord()[0],this.motionControl.getDestinationCoord()[1]);
				
					this.visual.addDebugLog2(DataChecker.moveInfo,this.motionControl.getDistance(),this.motionControl.getRotation(),this.motionControl.getTrigo());
					this.visual.addDebugLog2(DataChecker.movePulses,distance,rotation,this.motionControl.getTrigo());
					/**************/
				
					//Send move command...
					//Receive ack...
					
					/***Retrieve new robot's position***/
					if (Base.DEBUG) System.out.println("(executeRoutine1) distance = "+df.format(distance)+", rotation = "+df.format(rotation)+", trigo = "+motionControl.getTrigo());
					this.rover.setNewPosition(distance, rotation, (int) trigo);
					this.visual.addDebugLog2(DataChecker.newPosition,this.rover.getX(),this.rover.getY(),this.rover.getr());
					this.rover.storePosition();
					
					this.visual.updateDrawing(this.rover.getPositions(), this.nodes.getLandmarks(), null, null, this.motionControl.getPolygons());
					/************************************/
					
					if(Base.DEBUG) {
						System.out.println();
						Thread.sleep(200);
					} else {
						Thread.sleep(100);
					}
				}
			}
			
			//reinitialise robot
			this.rover.setPosition(-20, 30, 0);
			this.rover.storeErrorPosition();
			
			//reinitialise landmarks
			this.nodes.reinitialiseLandmarks();
			
			//initialise error (recording or pool)
			int step = 0;
			//initialise error pool
			this.storeError.genrateErrorPool();
			
			/*********************motion plan - error********************/
			while (euclideanDist(this.rover.getX(),this.rover.getY(),Base.finalDestination[0],Base.finalDestination[1])>3) { //end
				if (!this.simulation){
					Thread.sleep(1000);
				} else {
					if (Base.DEBUG) System.out.println("(executeRoutine1) Motion Plan - error");
					this.visual.addDebugLog2(DataChecker.initialPosition, this.rover.getX(),this.rover.getY(),this.rover.getr() );
					ListIterator<double[]> landmarkList = this.generatedLandmarks.listIterator();
					while (landmarkList.hasNext()) {
						double[] landmark = landmarkList.next();
						if ((euclideanDist(this.rover.getX(),this.rover.getY(),landmark[1],landmark[2])<=DataChecker.communicationRange)&&(!this.nodes.knownLandmark((int)landmark[0], landmark[1], landmark[2]))) this.nodes.storeLandmark((int)landmark[0], landmark[1], landmark[2]);	
					}
					
					for (int j=0;j<this.nodes.size();j++){
						tempError = this.nodes.realLandmarks.get(j);
						//Receiving nodes' coord
						coordIndex = (int)tempError[0];
						receiveCoord[0] = tempError[1];
						receiveCoord[1] = tempError[2];
					
						//Apply Anti-SLAM error
						antiSlamedCoord = receiveCoord;
					
						//Apply error
						double err = (this.storeError.getErrorFromPool(step))[1];
						if (Base.DEBUG) System.out.println("(executeRoutine1) Used error "+err+ " during step "+step);
						errCoord = this.mistaker.applyErrorFromPool(err, this.rover.getX(), this.rover.getY(), antiSlamedCoord[0], antiSlamedCoord[1]);
						this.visual.addDebugLog2(DataChecker.error, coordIndex,receiveCoord[0],receiveCoord[1],errCoord[0],errCoord[1]);
						step++;
						
						//Apply EKF - not supported /!\/!\/!\/!\
						finalCoord = errCoord;
						//Store final Coord
						this.nodes.replaceLandmark(coordIndex, finalCoord[0], finalCoord[1]);
						tempError = null;
					}
					
					//Display all landmarks
					li = this.nodes.getLandmarks().listIterator();
					while(li.hasNext()){
						tempLandmark = li.next();
						this.visual.addDebugLog2(DataChecker.landmark,tempLandmark[0],this.nodes.size(),tempLandmark[1],tempLandmark[2]);
					}
				
					//Apply EKF
					
					if (Base.DEBUG) System.out.println("(executeRoutine1) We have "+this.nodes.size()+" landmarks");
					this.motionControl.getNextPulses(this.rover.getX(),this.rover.getY(),this.rover.getr(),this.nodes);
					distance = this.motionControl.getPulse_dist();
					rotation = this.motionControl.getPulse_rot();
					trigo = this.motionControl.getTrigo();
				
					/*****Logs*****/
					this.visual.addDebugLog2(DataChecker.destination,this.motionControl.getDestinationCoord()[0],this.motionControl.getDestinationCoord()[1]);
				
					this.visual.addDebugLog2(DataChecker.moveInfo,this.motionControl.getDistance(),this.motionControl.getRotation(),this.motionControl.getTrigo());
					this.visual.addDebugLog2(DataChecker.movePulses,distance,rotation,this.motionControl.getTrigo());
					/**************/
				
					//Send move command...
					//Receive ack...
					
					/***Retrieve new robot's position***/
					if (Base.DEBUG) System.out.println("(executeRoutine1) distance = "+df.format(distance)+", rotation = "+df.format(rotation)+", trigo = "+motionControl.getTrigo());
					this.rover.setNewPosition(distance, rotation, (int) trigo);
					this.visual.addDebugLog2(DataChecker.newPosition,this.rover.getX(),this.rover.getY(),this.rover.getr());
					this.rover.storeErrorPosition();
					
					this.visual.updateDrawing(this.rover.getPositions(), this.nodes.getLandmarks(), this.rover.getErrorPositions(), this.rover.getEstimatePositions(),this.motionControl.getPolygons());
					/************************************/
					
					if(Base.DEBUG) {
						System.out.println();
						Thread.sleep(200);
					} else {
						Thread.sleep(100);
					}
				}
			}
			
			//reinitialise robot
			this.rover.setPosition(-20, 30, 0);
			this.rover.storeEstimatePosition();
			
			//reinitialise landmarks
			this.nodes.reinitialiseLandmarks();
			
			//initialise error reading (from recording or pool)
			step = 0;
			
			/*********************motion plan - error - ekf**************/
			while (euclideanDist(this.rover.getX(),this.rover.getY(),Base.finalDestination[0],Base.finalDestination[1])>3) { //end
				if (!this.simulation){
					Thread.sleep(1000);
				} else {
					if (Base.DEBUG) System.out.println("(executeRoutine1) Motion Plan - error - ekf");
					this.visual.addDebugLog2(DataChecker.initialPosition, this.rover.getX(),this.rover.getY(),this.rover.getr() );
					ListIterator<double[]> landmarkList = this.generatedLandmarks.listIterator();
					while (landmarkList.hasNext()) {
						double[] landmark = landmarkList.next();
						if ((euclideanDist(this.rover.getX(),this.rover.getY(),landmark[1],landmark[2])<=DataChecker.communicationRange)&&(!this.nodes.knownLandmark((int)landmark[0], landmark[1], landmark[2]))) this.nodes.storeLandmark((int)landmark[0], landmark[1], landmark[2]);	
					}
					
					//
					for (int j=0;j<this.nodes.size();j++){
						tempError = this.nodes.realLandmarks.get(j);
						//Receiving nodes' coord
						coordIndex = (int)tempError[0];
						receiveCoord[0] = tempError[1];
						receiveCoord[1] = tempError[2];
					
						//Apply Anti-SLAM error
						antiSlamedCoord = receiveCoord;
						
						
						//Apply error from stored error
						double err = (this.storeError.getErrorFromPool(step))[1];
						if (Base.DEBUG) System.out.println("(executeRoutine1) Used error "+err+ " during step "+step);
						errCoord = this.mistaker.applyErrorFromPool(err, this.rover.getX(), this.rover.getY(), antiSlamedCoord[0], antiSlamedCoord[1]);
						this.visual.addDebugLog2(DataChecker.error, coordIndex,receiveCoord[0],receiveCoord[1],errCoord[0],errCoord[1]);
						step++;
						
						//Apply EKF - not supported /!\/!\/!\/!\
						finalCoord = errCoord;
						//Store final Coord
						this.nodes.replaceLandmark(coordIndex, finalCoord[0], finalCoord[1]);
						tempError = null;
					}
					
					//Display all landmarks
					li = this.nodes.getLandmarks().listIterator();
					while(li.hasNext()){
						tempLandmark = li.next();
						this.visual.addDebugLog2(DataChecker.landmark,tempLandmark[0],this.nodes.size(),tempLandmark[1],tempLandmark[2]);
					}
				
					//Apply EKF
					this.filter.setMotion(this.rover.getX(), this.rover.getY(), 0, 0);
			        this.filter.initMeasurement(this.nodes.getLandmarks());
			        this.filter.calcul();
			        this.rover.setPosition(this.filter.getMEstimate()[0][0], this.filter.getMEstimate()[1][0], 0);
			        this.rover.storeEstimatePosition();
			        this.visual.addDebugLog2(DataChecker.ekf,this.rover.getX(),this.rover.getY());
					
					if (Base.DEBUG) System.out.println("(executeRoutine1) We have "+this.nodes.size()+" landmarks");
					this.motionControl.getNextPulses(this.rover.getX(),this.rover.getY(),this.rover.getr(),this.nodes);
					distance = this.motionControl.getPulse_dist();
					rotation = this.motionControl.getPulse_rot();
					trigo = this.motionControl.getTrigo();
				
					/*****Logs*****/
					this.visual.addDebugLog2(DataChecker.destination,this.motionControl.getDestinationCoord()[0],this.motionControl.getDestinationCoord()[1]);
				
					this.visual.addDebugLog2(DataChecker.moveInfo,this.motionControl.getDistance(),this.motionControl.getRotation(),this.motionControl.getTrigo());
					this.visual.addDebugLog2(DataChecker.movePulses,distance,rotation,this.motionControl.getTrigo());
					/**************/
				
					//Send move command...
					//Receive ack...
					
					/***Retrieve new robot's position***/
					if (Base.DEBUG) System.out.println("(executeRoutine1) distance = "+df.format(distance)+", rotation = "+df.format(rotation)+", trigo = "+motionControl.getTrigo());
					this.rover.setNewPosition(distance, rotation, (int) trigo);
					this.visual.addDebugLog2(DataChecker.newPosition,this.rover.getX(),this.rover.getY(),this.rover.getr());
					this.rover.storeEstimatePosition();
					
					this.visual.updateDrawing(this.rover.getPositions(), this.nodes.getLandmarks(), this.rover.getErrorPositions(), this.rover.getEstimatePositions(), this.motionControl.getPolygons());
					/************************************/
					
					if(Base.DEBUG) {
						System.out.println();
						Thread.sleep(200);
					} else {
						Thread.sleep(100);
					}
				}
			}
			
			/************motion plan - antislam - error - ekf*************/
			while (euclideanDist(this.rover.getX(),this.rover.getY(),Base.finalDestination[0],Base.finalDestination[1])>3) { //end
				if (!this.simulation){
					Thread.sleep(1000);
				} else {
					if (Base.DEBUG) System.out.println("(executeRoutine1) Motion Plan - antislam - error - ekf");
					this.visual.addDebugLog2(DataChecker.initialPosition, this.rover.getX(),this.rover.getY(),this.rover.getr() );
					ListIterator<double[]> landmarkList = this.generatedLandmarks.listIterator();
					while (landmarkList.hasNext()) {
						double[] landmark = landmarkList.next();
						if ((euclideanDist(this.rover.getX(),this.rover.getY(),landmark[1],landmark[2])<=DataChecker.communicationRange)&&(!this.nodes.knownLandmark((int)landmark[0], landmark[1], landmark[2]))) this.nodes.storeLandmark((int)landmark[0], landmark[1], landmark[2]);	
					}
					
					//
				}
			}
			
		} catch (Exception e) {
			e.printStackTrace();
		}
	}
	
	public void executeRoutine2() throws Exception {
		
		/*initialize all components*/
		initMain();
		initEkf();
		initMotion();		
		initError();	
		initAntiSlam();
		
		if (this.completeSimulation) {
			this.executeRoutine1();
			return;
		}
		
		double [] tempError = null;
		double [] tempLandmark = null;
		double [] receiveCoord = new double[2];
		double [] antiSlamedCoord = new double[2];
		double [] errCoord, finalCoord;
		ListIterator<double[]> li;
		double distance, rotation, trigo;
		int coordIndex = 0;
		DecimalFormat df = new DecimalFormat("#####.##"); 
		
		try {
			/*********************main test********************/
			while(true){
				if (!this.simulation){
					Thread.sleep(1000);
				} else {
				this.visual.addDebugLog2(DataChecker.initialPosition, this.rover.getX(),this.rover.getY(),this.rover.getr() );
				if (this.scenario == 1) {
					if ((this.rover.getX()==-10)&&(this.nodes.size()==0)) this.nodes.storeLandmark(1, 0, 20);
					if ((this.rover.getX()>=-1)&&(this.rover.getX()<=1)&&(this.nodes.size()==1)) {
						this.nodes.storeLandmark(2, 20, 20);
						this.nodes.storeLandmark(4, 0, 0);
						this.nodes.storeLandmark(5, 10, 10);
					}
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),20,0)<=22)&&(this.nodes.size()==4)){
						this.nodes.storeLandmark(3, 20, 0);
					}
				} else if (this.scenario == 2){
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),0,30)<=20)&&(!this.nodes.knownLandmark(1, 0, 30))) this.nodes.storeLandmark(1, 0, 30);
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),0,0)<=20)&&(!this.nodes.knownLandmark(4, 0, 0))) this.nodes.storeLandmark(4, 0, 0);
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),15,15)<=20)&&(!this.nodes.knownLandmark(5, 15, 15))) this.nodes.storeLandmark(5, 15, 15);
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),30,30)<=20)&&(!this.nodes.knownLandmark(2, 30, 30))) this.nodes.storeLandmark(2, 30, 30);
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),30,0)<=20)&&(!this.nodes.knownLandmark(3, 30, 0))) this.nodes.storeLandmark(3, 30, 0);
					//end
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),Base.finalDestination[0],Base.finalDestination[1])<=3)) return;	
				} else if (this.scenario == 3){
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),0,30)<=20)&&(!this.nodes.knownLandmark(1, 0, 30))) this.nodes.storeLandmark(1, 0, 30);
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),2,3)<=20)&&(!this.nodes.knownLandmark(4, 2, 3))) this.nodes.storeLandmark(4, 2, 3);
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),19,10)<=20)&&(!this.nodes.knownLandmark(5, 19, 10))) this.nodes.storeLandmark(5, 19, 10);
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),22,30)<=20)&&(!this.nodes.knownLandmark(2, 22, 30))) this.nodes.storeLandmark(2, 22, 30);
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),34,4)<=20)&&(!this.nodes.knownLandmark(3, 34, 4))) this.nodes.storeLandmark(3, 34, 4);
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),5,0)<=20)&&(!this.nodes.knownLandmark(7, 5,0))) this.nodes.storeLandmark(7, 5,0);
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),36,25)<=20)&&(!this.nodes.knownLandmark(6, 36, 25))) this.nodes.storeLandmark(6, 36, 25);
					//end
					if ((euclideanDist(this.rover.getX(),this.rover.getY(),Base.finalDestination[0],Base.finalDestination[1])<=5)) this.stopRoutine();	
				}
		        for (int j=0;j<this.nodes.size();j++){
		        	tempError = this.nodes.realLandmarks.get(j);
					//Receiving nodes' coord
					coordIndex = (int)tempError[0];
					receiveCoord[0] = tempError[1];
					receiveCoord[1] = tempError[2];
					//Apply Anti-SLAM error
					if ((this.antiSlam)&&(this.scenario==1)){
						antiSlamedCoord = this.counterSlam.applyAntiSlam(coordIndex,receiveCoord,this.rover.getX(), this.rover.getY());
						this.visual.addDebugLog2(DataChecker.antiSlam, coordIndex,receiveCoord[0],receiveCoord[1],antiSlamedCoord[0],antiSlamedCoord[1]);
					} else {
						antiSlamedCoord = receiveCoord;
					}
					//Apply error
					if (this.enableError){
						errCoord = this.mistaker.applyError(this.rover.getX(), this.rover.getY(), antiSlamedCoord[0], antiSlamedCoord[1]);
						this.visual.addDebugLog2(DataChecker.error, coordIndex,receiveCoord[0],receiveCoord[1],errCoord[0],errCoord[1]);
					} else {
						errCoord = antiSlamedCoord;
					}
					//Store final Coord
					finalCoord = errCoord;
					this.nodes.replaceLandmark(coordIndex, finalCoord[0], finalCoord[1]);
					tempError = null;
				}
				//Display all landmarks
		        li = this.nodes.getLandmarks().listIterator();
		        while(li.hasNext()){
		        	tempLandmark = li.next();
		        	this.visual.addDebugLog2(DataChecker.landmark,tempLandmark[0],this.nodes.size(),tempLandmark[1],tempLandmark[2]);
		        }
				
				//Apply EKF
		        /*if (this.enableEkf){
		        	this.filter.setMotion(this.rover.getX(), this.rover.getY(), 0, 0);
			        this.filter.initMeasurement(this.nodes.getLandmarks());
			        this.filter.calcul();
			        this.rover.setPosition(this.filter.getMEstimate()[0][0], this.filter.getMEstimate()[1][0], 0);
			        this.rover.storePosition();
			        this.visual.addDebugLog2(DataChecker.ekf,this.rover.getX(),this.rover.getY(),0, 0);
			        if (Base.DEBUG) System.out.println("(executeRoutine2) Exit of EKF x: "+df.format(this.rover.getX())+ " Y: "+df.format(this.rover.getY()));
		        }*/
		        
				if (Base.DEBUG) System.out.println("(executeRoutine2) We have "+this.nodes.size()+" landmarks");
				this.motionControl.getNextPulses(this.rover.getX(),this.rover.getY(),this.rover.getr(),this.nodes);
				distance = this.motionControl.getPulse_dist();
				rotation = this.motionControl.getPulse_rot();
				trigo = this.motionControl.getTrigo();
				
				/*****Logs*****/
				this.visual.addDebugLog2(DataChecker.destination,this.motionControl.getDestinationCoord()[0],this.motionControl.getDestinationCoord()[1]);
				
				this.visual.addDebugLog2(DataChecker.moveInfo,this.motionControl.getDistance(),this.motionControl.getRotation(),this.motionControl.getTrigo());
				this.visual.addDebugLog2(DataChecker.movePulses,distance,rotation,this.motionControl.getTrigo());
				/**************/
				
				/*
				//Send move command...
				MotionCommand motionCmd = new MotionCommand(16, (int)distance, (int)rotation, (int)trigo);
				transmission.txMotionCom(motionCmd);
				
				
				//Receive ack...
				Thread.sleep(1000);
				MotionCommand motionAck = reception.getMotionAck();
				if (motionAck != null) {
					if ((motionAck.getDistancePulse() == distance) && (motionAck.getRotationPulse() == rotation)) {
						if (Base.DEBUG) System.out.println("(executeRoutine2) Received ACK");
					} else {
						if (Base.DEBUG) System.out.println("(executeRoutine2) ACK not received retransmitting");
						continue;
					}
				} else {
					if (Base.DEBUG) System.out.println("(executeRoutine2) ACK not received retransmitting");
					continue;
				}
				*/
				
				/***Retrieve new robot's position***/
				if (Base.DEBUG) System.out.println("(executeRoutine2) distance = "+df.format(distance)+", rotation = "+df.format(rotation)+", trigo = "+trigo);
				this.rover.setNewPosition(distance, rotation, (int) trigo);
				this.visual.addDebugLog2(DataChecker.newPosition,this.rover.getX(),this.rover.getY(),this.rover.getr());
				this.rover.storePosition();
				if (Base.DEBUG) System.out.println("(executeRoutine2) After motion x: "+df.format(this.rover.getX())+ " y: "+df.format(this.rover.getY()));
				
				this.visual.updateDrawing(this.rover.getPositions(), this.nodes.getLandmarks(), null, this.rover.getEstimatePositions(), this.motionControl.getPolygons());
				/************************************/
				
				if(Base.DEBUG) {
					System.out.println();
					Thread.sleep(200);
				} else {
					Thread.sleep(100);
				}
				}
			}
		} catch (Exception e) {
			e.printStackTrace();
		}
	}
		
	private double euclideanDist(double x1, double y1, double x2, double y2){
		return(Math.sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1)));
	}

}
